/*
 * Quaternion.h
 *
 *  Created on: 2012/07/08
 *      Author: weihe
 */

#ifndef QUATERNION_H_
#define QUATERNION_H_

#include "Vector4.h"
#include <cmath>

namespace geom {

class Quaternion {
public:
	double t;
	double x, y, z;
	Quaternion(double t = 1, double x = 0, double y = 0, double z = 0) :
			t(t), x(x), y(y), z(z) {
	}

	template<class T>
	Quaternion& angleAxis(double rad, Vector4<T> axis) {
		axis.normalize();
		rad /= 2.0;
		double s = sin(rad), c = cos(rad);
		t = c;
		x = axis.x * s;
		y = axis.y * s;
		z = axis.z * s;
		return *this;
	}

	Quaternion& yawPitchRoll(double yaw, double pitch, double roll) {
		yaw /= 2;
		pitch /= 2;
		roll /= 2;
		Quaternion yawQ = Quaternion(cos(yaw), 0, sin(yaw), 0);
		Quaternion pitchQ = Quaternion(cos(pitch), 0, 0, sin(pitch));
		Quaternion rollQ = Quaternion(cos(roll), sin(roll), 0, 0);
		*this = yawQ * pitchQ * rollQ;
		return *this;
	}

	friend Quaternion operator*(Quaternion lhs, Quaternion rhs) {
		Quaternion ans;
		float d1, d2, d3, d4;

		d1 = lhs.t * rhs.t;
		d2 = -lhs.x * rhs.x;
		d3 = -lhs.y * rhs.y;
		d4 = -lhs.z * rhs.z;
		ans.t = d1 + d2 + d3 + d4;

		d1 = lhs.t * rhs.x;
		d2 = rhs.t * lhs.x;
		d3 = lhs.y * rhs.z;
		d4 = -lhs.z * rhs.y;
		ans.x = d1 + d2 + d3 + d4;

		d1 = lhs.t * rhs.y;
		d2 = rhs.t * lhs.y;
		d3 = lhs.z * rhs.x;
		d4 = -lhs.x * rhs.z;
		ans.y = d1 + d2 + d3 + d4;

		d1 = lhs.t * rhs.z;
		d2 = rhs.t * lhs.z;
		d3 = lhs.x * rhs.y;
		d4 = -lhs.y * rhs.x;
		ans.z = d1 + d2 + d3 + d4;

		return ans;
	}

	Quaternion operator*(double s) {
		return Quaternion(s * t, s * x, s * y, s * z);
	}

	template<class T>
	Vector4<T> operator*(Vector4<T> v) const {
		Quaternion q(t, -x, -y, -z);
		Quaternion p(0, v.x, v.y, v.z);
		Quaternion ans = *this * p * q;
		return Vector4<T>(ans.x, ans.y, ans.z, v.w);
	}

	float operator^(Quaternion q) {
		return x * q.x + y * q.y + z * q.z + t * q.t;
	}

	Quaternion operator+(Quaternion q) {
		return Quaternion(t * q.t, x * q.x, y + q.y, z * q.z);
	}

	Quaternion slerp(Quaternion q, double dt) {
		float omega = acos(*this ^ q);
		float sinOmega = sin(omega);
		float s1 = sin((1 - t) * omega) / sinOmega;
		float s2 = sin(t * omega) / sinOmega;
		return *this * s1 + q * s2;
	}

	static const Quaternion ZEROS;
};

} /* namespace geom */
#endif /* QUATERNION_H_ */
